import lejos.nxt.comm.NXTConnection;
import lejos.pc.comm.*;
import java.io.*;
import java.util.ArrayList;
import java.util.Enumeration;
import java.util.Hashtable;

/**
 * This is a PC sample. It connects to the NXT, and then
 * sends an integer and waits for a reply, 100 times.
 * 
 * Compile this program with javac (not nxjc), and run it 
 * with java.
 * 
 * You need pccomm.jar and bluecove.jar on the CLASSPATH. 
 * On Linux, you will also need bluecove-gpl.jar on the CLASSPATH.
 * 
 * Run the program by:
 * 
 *   java BTSend 
 * 
 * Your NXT should be running a sample such as BTReceive or
 * SignalTest. Run the NXT program first until it is
 * waiting for a connection, and then run the PC program. 
 * 
 * @author Lawrie Griffiths
 *
 */
public class BTSendNew {	
	
	private static final String JAN = "btspp://00:16:53:07:1C:A0";
	private static final String FOO = "btspp://";
	private static final String BAR = "btspp://";

	private static Hashtable<String, NXTConnector> connections = new Hashtable<String, NXTConnector>();
	private static NXTConnector currentConn = new NXTConnector();
	
	private static void connectToRobot(String robotName) {
		System.out.println("Connecting to robot " + robotName);

		// Connect to any NXT over Bluetooth
		//boolean connected = conn.connectTo("btspp://00:16:53:07:1C:A0");
	    
	    if (!connections.containsKey(robotName)) {
	    	connections.put(robotName, new NXTConnector());
	    }
	     
	    currentConn = connections.get(robotName);
	    //currentConn.getNXTComm().
	    
	    boolean connected = currentConn.connectTo(robotName);
	    //boolean connected = conn.connectTo(robotName);
		
		if (!connected) {
			System.err.println("Failed to connect to: "+robotName);
			System.exit(1);
		}
	}
	
	private static void connectToAll() {
		connectToRobot("JAN");
		connectToRobot("FOO");
		connectToRobot("BAR");
	}
	
	public static void main(String[] args) {
		System.out.println("Lego terminal");
		System.out.println("-------------");
		System.out.println("Available robots: ");
		System.out.println("JAN");
		System.out.println("FOO");
		System.out.println("BAR");
		System.out.print("Choose robot: ");
		
		//  open up standard input
		BufferedReader br = new BufferedReader(new InputStreamReader(System.in));
	    
	    String robotName = null;
	    
	    try {robotName = br.readLine();} 
	    catch (IOException ioe) {
		   System.out.println("IO error");
		   System.exit(1);
	    }
	
	    connectToAll();
	    
	    Enumeration<NXTConnector> e = connections.elements();
	    
	    /*while (e.hasMoreElements()) {
	    	CommMessage cm = CommMessage.CreateMessage(e.nextElement());
	    }*/
		
		DataOutputStream dos = currentConn.getDataOut();
		DataInputStream dis = currentConn.getDataIn();
				
		for(int i=0;i<100;i++) {
			try {
				System.out.println("Sending " + (i*30000));
				dos.writeInt((i*30000));
				dos.flush();			
				
			} catch (IOException ioe) {
				System.out.println("IO Exception writing bytes:");
				System.out.println(ioe.getMessage());
				break;
			}
			
			try {
				System.out.println("Received " + dis.readInt());
			} catch (IOException ioe) {
				System.out.println("IO Exception reading bytes:");
				System.out.println(ioe.getMessage());
				break;
			}
		}
		
		try {
			dis.close();
			dos.close();
			currentConn.close();
		} catch (IOException ioe) {
			System.out.println("IOException closing connection:");
			System.out.println(ioe.getMessage());
		}
	}
}
